#include "app_sensor_process.h"

#include "common_debug.h"
#include "common_util.h"

#include <string.h>

#define SENSOR_FRAME_FREQ_1 0x20
#define SENSOR_FRAME_FREQ_2 0x21
#define SENSOR_FRAME_FREQ_3 0x22
#define SENSOR_FRAME_FREQ_4 0x23
#define SENSOR_FRAME_FREQ_5 0x24

static void app_sensor_process_handle_sensor_frame(uint32_t sensor_id, uint8_t frame_seq,
                                                   msg_frame_t *frame);
static void app_sensor_process_start_new_packet(sensor_packet_t *packet, msg_frame_t *frame);
static void app_sensor_process_add_frame_to_packet(sensor_packet_t *packet, msg_frame_t *frame);
static bool app_sensor_process_validate_packet_crc(sensor_packet_t *packet);
static void app_sensor_process_reset_packet(sensor_packet_t *packet);
static void app_sensor_process_store_complete_packet(uint32_t sensor_id, sensor_packet_t *packet);
static void app_sensor_prcoess_ready_crc_data(sensor_packet_t *packet, uint8_t *data);
static void app_sensor_process_get_press_value(uint8_t *data, uint16_t data_len,
                                               uint16_t *press_values_list);

sensor_manager_t sensor_manager;

/**
 * @brief 处理传感器消息队列中的数据
 *
 */
void app_sensor_process_handle(void)
{
    while (!common_queue_is_empty(&sensor_msg_queue)) {
        msg_frame_t msg_frame;
        common_queue_pop(&sensor_msg_queue, &msg_frame);

        /* 解析传感器类型和帧序号 */
        uint32_t sensor_id = msg_frame.msg_id;
        uint8_t  frame_seq = msg_frame.data[0]; /* 帧首字节为帧序号 */

        /* 校验传感器ID范围 */
        if (sensor_id < SENSOR_TYPE_PK1B_BACK || sensor_id > SENSOR_TYPE_PK1B_LEFT) {
            continue;
        }

        /* 校验帧序列号范围 */
        if (frame_seq < SENSOR_FRAME_FREQ_1 || frame_seq > SENSOR_FRAME_FREQ_5) {
            continue;
        }

        /* 处理该帧数据 */
        app_sensor_process_handle_sensor_frame(sensor_id, frame_seq, &msg_frame);
    }
}

/**
 * @brief 处理单帧传感器数据
 *
 * @param sensor_id     传感器ID
 * @param frame_seq     帧序号
 * @param frame         传感器数据帧
 */
static void app_sensor_process_handle_sensor_frame(uint32_t sensor_id, uint8_t frame_seq,
                                                   msg_frame_t *frame)
{
    uint8_t          sensor_index = sensor_id - SENSOR_BASE_ID - 1; /* -1是因为数组是从0开始计算的 */
    sensor_packet_t *packet       = &sensor_manager.packets[sensor_index];

    /* 通过状态机处理 */
    switch (packet->state) {
    case PACKET_STATE_IDLE:
        if (frame_seq == SENSOR_FRAME_FREQ_1) {
            /* 开始新的数据包 */
            app_sensor_process_start_new_packet(packet, frame);
        }
        break;

    case PACKET_STATE_RECEIVING:
        if (frame_seq == packet->expected_seq) {
            /* 接收期望帧 */
            app_sensor_process_add_frame_to_packet(packet, frame);
            if (packet->frame_count == SENSOR_PACKET_FRAME_SIZE) {
                /* 进行CRC校验 */
                if (app_sensor_process_validate_packet_crc(packet)) {
                    packet->state = PACKET_STATE_COMPLETE;
                }
                else {
                    packet->state = PACKET_STATE_ERROR;
                }

                // TODO 完整的一包数据，待处理成压力
                app_sensor_process_store_complete_packet(sensor_id, packet);
                app_sensor_process_get_press_value(sensor_manager.sensor_data_list[sensor_index].data,
                                                   SENSOR_PACKET_FRAME_SIZE * SENSOR_MSG_FRAME_SIZE,
                                                   sensor_manager.sensor_press_values_list[sensor_index]);

                /* 清空对应传感器的packet包存储器 */
                app_sensor_process_reset_packet(packet);
            }
        }
        else {
            /* 帧序号不连续，重置数据包 */
            if (frame_seq == SENSOR_FRAME_FREQ_1) {
                app_sensor_process_start_new_packet(packet, frame);
            }
            else {
                app_sensor_process_reset_packet(packet);
            }
        }
        break;

    default:
        /* 其他状态，重置数据包 */
        app_sensor_process_reset_packet(packet);
        break;
    }
}

/**
 * @brief 从传感器数据中提取压力值
 *
 * @param data              传感器数据
 * @param data_len          数据长度
 * @param press_values_list 压力值列表
 */
static void app_sensor_process_get_press_value(uint8_t *data, uint16_t data_len,
                                               uint16_t *press_values_list)
{
    /* 定义常量 */
    static const uint8_t PRESSURE_DATA_SIZE = 2 * 14; /* 14个压力值，每个2字节 */
    static const uint8_t DATA_START_OFFSET  = 6;      /* 数据起始偏移量 */
    static const uint8_t DATA_END_OFFSET    = 2;      /* 数据结束偏移量(CRC+无效数据) */

    uint8_t pressure_data[PRESSURE_DATA_SIZE] = { 0 };
    uint8_t pressure_data_index               = 0;

    /* 第一步：从原始数据中提取有效压力数据，跳过帧序号 */
    for (uint8_t i = DATA_START_OFFSET; i < data_len - DATA_END_OFFSET; ++i) {
        /* 跳过每帧的首字节(帧序号) */
        if (i % SENSOR_MSG_FRAME_SIZE == 0) {
            continue;
        }

        /* 防止数组越界 */
        if (pressure_data_index < PRESSURE_DATA_SIZE) {
            pressure_data[pressure_data_index++] = data[i];
        }
    }

    /* 第二步：将压力数据转换为16位压力值(小端格式) */
    uint8_t pressure_value_index = 0;
    for (uint8_t i = 0; i < pressure_data_index && i + 1 < PRESSURE_DATA_SIZE; i += 2) {
        /* 小端格式：低字节在前，高字节在后 */
        press_values_list[pressure_value_index++] = ((pressure_data[i + 1] << 8) | pressure_data[i]) / 10;
    }
}

/**
 * @brief 准备CRC数据
 *
 * @param packet 传感器数据包
 * @param data   数据指针
 */
static void app_sensor_prcoess_ready_crc_data(sensor_packet_t *packet, uint8_t *data)
{
    uint8_t data_idx = 0;
    for (uint8_t frame_idx = 0; frame_idx < SENSOR_PACKET_FRAME_SIZE; ++frame_idx) {
        for (uint8_t byte_idx = 1; byte_idx < SENSOR_MSG_FRAME_SIZE; ++byte_idx) {
            if (frame_idx == SENSOR_PACKET_FRAME_SIZE - 1 && byte_idx == SENSOR_MSG_FRAME_SIZE - 2) {
                return;
            }
            data[data_idx++] = packet->data[frame_idx][byte_idx];
        }
    }
}

/**
 * @brief 校验传感器数据包的CRC校验值
 *
 * @param   packet  传感器数据包
 * @return  true    校验成功
 * @return  false   校验失败
 */
static bool app_sensor_process_validate_packet_crc(sensor_packet_t *packet)
{
    uint8_t crc_data[7 + 7 + 7 + 7 + 5] = { 0 }; // TODO 待完善吧，有点丑
    app_sensor_prcoess_ready_crc_data(packet, crc_data);

    uint8_t crc8 = common_util_crc8_calculate(crc_data, 7 + 7 + 7 + 7 + 5);

    /* 校验CRC8 */
    if (crc8 != packet->data[SENSOR_PACKET_FRAME_SIZE - 1][SENSOR_MSG_FRAME_SIZE - 2]) {
        return false;
    }

    return true;
}

/**
 * @brief 数据就绪后存储数据
 *
 * @param sensor_id 传感器ID
 * @param packet    传感器数据包
 */
static void app_sensor_process_store_complete_packet(uint32_t sensor_id, sensor_packet_t *packet)
{
    uint8_t index = sensor_id - SENSOR_BASE_ID - 1;
    memcpy(&sensor_manager.sensor_data_list[index].data, packet->data,
           SENSOR_PACKET_FRAME_SIZE * SENSOR_MSG_FRAME_SIZE);
    sensor_manager.sensor_data_list[index].timestamp = common_util_get_tick();
    sensor_manager.sensor_data_list[index].is_ready  = true;
}

/**
 * @brief 重置传感器数据包状态
 *
 * @param packet 传感器数据包
 */
static void app_sensor_process_reset_packet(sensor_packet_t *packet)
{
    memset(packet, 0, sizeof(sensor_packet_t));
    packet->state        = PACKET_STATE_IDLE;
    packet->timestamp    = 0;
    packet->frame_count  = 0;
    packet->expected_seq = SENSOR_FRAME_FREQ_1;
}

/**
 * @brief 添加帧到数据包
 *
 * @param packet 传感器数据包
 * @param frame  传感器数据帧
 */
static void app_sensor_process_add_frame_to_packet(sensor_packet_t *packet, msg_frame_t *frame)
{
    memcpy(packet->data[packet->frame_count], frame->data, SENSOR_MSG_FRAME_SIZE);
    ++packet->frame_count;
    ++packet->expected_seq;
}

/**
 * @brief 开始新的数据包接收
 *
 * @param packet 传感器数据包
 * @param frame  传感器数据帧
 */
static void app_sensor_process_start_new_packet(sensor_packet_t *packet, msg_frame_t *frame)
{
    memset(packet, 0, sizeof(sensor_packet_t));
    memcpy(packet->data[0], frame->data, SENSOR_MSG_FRAME_SIZE);
    packet->frame_count  = 1;
    packet->expected_seq = SENSOR_FRAME_FREQ_2;
    packet->state        = PACKET_STATE_RECEIVING;
    packet->timestamp    = common_util_get_tick();
}
